#include "CCamera.h"
#include "cvxVideo.h"
#include <opencv2\calib3d\calib3d.hpp>
#include <time.h>
#include "cvxCapture.h"
#include "cvxIo.h"
#include "cvxSf.h"
#include "cvxCalib3d.h"
#include "nvmath.h"
#include "cvxMat.h"
#include <algorithm>
#include <numeric>

using namespace cvx;

//test pair camera from 

#if 0
//detect blur in the image
int main()
{
	//string path = string("save_image\\13\\org\\*.png");  10-org
	string path = string("10-org\\*.png");

	vector<std::string> fileNames;
	bool isOk = CvxIo::travel_dir(path.c_str(), fileNames);

	/*
	size_t found;
	found= path.find_last_of("/\\");
	string folder = path.substr(0, found);
	*/
	size_t found;
	found = path.find_last_of("/\\");
	string folder = path.substr(0, found);
	folder += string("\\");	

	vector<int> midVec;
	for (int i = 0; i<fileNames.size(); ++i)
	{
		string file = folder + fileNames[i];
		vector<Point2f> pts;
		IplImage *image = cvLoadImage(file.c_str(), 1);
		assert(image);
		bool isFound = CvxSF::GetChessboard(image, cvSize(15, 9), false, pts);
		if (isFound)
		{
			IplImage *grayImage = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
			cvCvtColor(image, grayImage, CV_BGR2GRAY);

			int x = pts[0].x;
			int y = pts[0].y;
			
			int bin[3] = {0};
			int step = 20;
			
			IplImage *cropImage = cvCreateImage(cvSize(step*2, step*2), IPL_DEPTH_8U, 1);
			cvSetImageROI(grayImage, cvRect(x-step, y-step, step*2, step*2));
			cvCopy(grayImage, cropImage);
			cvResetImageROI(grayImage);

			cvNormalize(cropImage, cropImage, 0, 255, CV_MINMAX);

			BwImage crop_img(cropImage);
			for (int k = 0; k<2*step; ++k )
			{
				for (int j = 0; j<2*step; ++j)
				{
					//	fprintf(stdout, "%d ", (int)gray_img[k][j]);
					int idx = crop_img[k][j]/85;
					if (idx == 3)
					{
						idx--;
					}
					bin[idx]++;
				}
			}
			midVec.push_back(bin[1]);
 		//	cvShowImage("crop", cropImage);
 		//	cvSaveImage("crop.png", cropImage);
			fprintf(stdout, "\n");
			fprintf(stdout, "%s %d %d %d\n", fileNames[i].c_str(),bin[0], bin[1], bin[2]);

 		//	cvWaitKey(0);			
			cvReleaseImage(&grayImage);
		}
	}
	int ave = std::accumulate(midVec.begin(), midVec.end(), 0) / midVec.size();
	fprintf(stdout, "ave_val = %d\n\n", ave);

//	return 1;

	ImageSequenceCapture cap = ImageSequenceCapture(path);

	CChessBoard board(cvSize(15, 9), 0.04);


	CCamera iCamera;
	iCamera.captureFeature(&cap, board, 12);
	iCamera.setCorners3d(board);
	// CV_CALIB_FIX_K3| CV_CALIB_FIX_K4|CV_CALIB_FIX_K5
	iCamera.calib(board, CV_CALIB_ZERO_TANGENT_DIST|CV_CALIB_FIX_K3, true);
	Mat intri = iCamera.getIntrinsicMatrix(); 
	Mat distortion = iCamera.getDistortionCoeffs(); 
	fprintf(stdout, "%d %f %f %f %f %f %f %f\n\n", 10, intri.at<double>(0, 0), intri.at<double>(1, 1), intri.at<double>(0, 2),
			intri.at<double>(1, 2), distortion.at<double>(0, 0), distortion.at<double>(1, 0),
			intri.at<double>(1, 1)/intri.at<double>(0, 0));

	cvWaitKey(0);
	return 1;

}
#endif

#if 0
//record reprojection error
int main()
{
	CCamera botC;
	CCamera topC;

	botC.loadCamera("save_image\\15_9\\bottom\\960\\bottom_960_cam.txt");
	topC.loadCamera("save_image\\15_9\\top\\960\\top_960_cam.txt");

	cv::Mat botMatrix = botC.getIntrinsicMatrix();
	cv::Mat botDistortion = botC.getDistortionCoeffs();
	cv::Mat topMatrix = topC.getIntrinsicMatrix();
	cv::Mat topDistortion = topC.getDistortionCoeffs();

	string botVideoFileName = string("video\\15_9\\bottom_23345300_15_9_960.avi");
	string topVideoFileName = string("video\\15_9\\top_23345209_15_9_960.avi");

	string videoFileName;
	vector<int> Idx;
	vector<vector<Point2f> > botChessboardPos;
	vector<vector<Point2f> > topChessboardPos;
	CvSize chessboardSize;
	CChessBoard board(cvSize(15, 9), 0.025);


	CvxSF::loadStereoChessboard(string("video\\15_9\\bot_top_960_ckb_pos.txt"), botVideoFileName, topVideoFileName, 
				Idx, chessboardSize, botChessboardPos, topChessboardPos);

	int testFrame = 1;
	FILE *pf = fopen("bot_960_reprojection_error.txt", "w");
	assert(pf);
	for (int i = 0;  i<botChessboardPos.size(); ++i)
	{
		double error = CCamera::GetReProjectionError(botMatrix, botDistortion, 									
						board._pts3d,
						botChessboardPos[i], 
						pf);

	}
	fclose(pf);

	return 1;
}

#endif

#if 0
int main()
{
	double tt = clock();

	//video\\9_6_avi\\left.avi
	//video\\9_6_mxf\\top5801.MXF
	//video\\9_6_mxf\\bottom5809.MXF
	string videoFile = string("video\\9_6_mxf\\bottom5809.MXF");
	CvCapture *cap  = cvCaptureFromFile(videoFile.c_str());  //left_9_6.avi  
	
	assert(cap);

	CChessBoard board(cvSize(9, 6), 0.025);

	CvxFileCapture *icap = new CvxFileCapture(cap, 24);
	//icap->SetProperty(CVX_FILE_CAP_PROP_FPS, 2);
	icap->SetProperty(CVX_FILE_CAP_FLIP, 1);

	CCamera iCamera;
	tt = clock();
	iCamera.captureFeature(icap, board, 3);
	iCamera.setCorners3d(board);
	// CV_CALIB_FIX_K3| CV_CALIB_FIX_K4|CV_CALIB_FIX_K5
	iCamera.calib(board, CV_CALIB_FIX_K4|CV_CALIB_FIX_K5, true);
	iCamera.saveText(videoFile, string("bottom.txt"));

	cout<<"calib cost time = "<<clock() - tt<<endl;

	cvWaitKey(0);
	return 1;
}

#endif
